#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>

class CameraDisplayNode
{
private:
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Subscriber rgb_sub_;
    image_transport::Subscriber depth_sub_;
    
    cv::Mat rgb_image_;
    cv::Mat depth_image_;
    bool has_rgb_data_;
    bool has_depth_data_;

public:
    CameraDisplayNode() : it_(nh_), has_rgb_data_(false), has_depth_data_(false)
    {
        // 订阅RGB图像话题
        rgb_sub_ = it_.subscribe("/camera/color/image_raw", 1, &CameraDisplayNode::rgbCallback, this);
        
        // 订阅深度图像话题
        depth_sub_ = it_.subscribe("/camera/depth/image_rect_raw", 1, &CameraDisplayNode::depthCallback, this);
        
        // 创建OpenCV窗口
        cv::namedWindow("松灵小车 - RGB图像", cv::WINDOW_AUTOSIZE);
        cv::namedWindow("松灵小车 - 深度图", cv::WINDOW_AUTOSIZE);
        
        ROS_INFO("相机显示节点已启动");
        ROS_INFO("正在等待相机数据...");
    }

    ~CameraDisplayNode()
    {
        cv::destroyAllWindows();
    }

    void rgbCallback(const sensor_msgs::Image::ConstPtr& msg)
    {
        try
        {
            cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
            rgb_image_ = cv_ptr->image;
            has_rgb_data_ = true;
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("RGB图像转换错误: %s", e.what());
        }
    }

    void depthCallback(const sensor_msgs::Image::ConstPtr& msg)
    {
        try
        {
            cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1);
            depth_image_ = cv_ptr->image;
            has_depth_data_ = true;
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("深度图像转换错误: %s", e.what());
        }
    }

    void displayImages()
    {
        if (has_rgb_data_)
        {
            // 在RGB图像上添加信息
            cv::Mat display_rgb = rgb_image_.clone();
            cv::putText(display_rgb, "SCOUT MINI - RGB Camera", 
                       cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, 
                       cv::Scalar(0, 255, 0), 2);
            cv::putText(display_rgb, "Resolution: " + std::to_string(rgb_image_.cols) + "x" + std::to_string(rgb_image_.rows), 
                       cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 0.7, 
                       cv::Scalar(255, 255, 255), 2);
            cv::putText(display_rgb, "Press 'q' to quit", 
                       cv::Point(10, display_rgb.rows - 20), cv::FONT_HERSHEY_SIMPLEX, 0.7, 
                       cv::Scalar(255, 255, 255), 2);
            
            cv::imshow("松灵小车 - RGB图像", display_rgb);
        }
        else
        {
            // 显示等待信息
            cv::Mat waiting_rgb = cv::Mat::zeros(480, 640, CV_8UC3);
            cv::putText(waiting_rgb, "等待RGB图像数据...", 
                       cv::Point(150, 240), cv::FONT_HERSHEY_SIMPLEX, 1, 
                       cv::Scalar(0, 255, 0), 2);
            cv::imshow("松灵小车 - RGB图像", waiting_rgb);
        }

        if (has_depth_data_)
        {
            // 将深度图转换为可视化格式
            cv::Mat depth_vis;
            cv::normalize(depth_image_, depth_vis, 0, 255, cv::NORM_MINMAX, CV_8U);
            cv::applyColorMap(depth_vis, depth_vis, cv::COLORMAP_JET);
            
            // 在深度图上添加信息
            cv::putText(depth_vis, "SCOUT MINI - Depth Camera", 
                       cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, 
                       cv::Scalar(255, 255, 255), 2);
            cv::putText(depth_vis, "Resolution: " + std::to_string(depth_image_.cols) + "x" + std::to_string(depth_image_.rows), 
                       cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 0.7, 
                       cv::Scalar(255, 255, 255), 2);
            cv::putText(depth_vis, "Press 'q' to quit", 
                       cv::Point(10, depth_vis.rows - 20), cv::FONT_HERSHEY_SIMPLEX, 0.7, 
                       cv::Scalar(255, 255, 255), 2);
            
            cv::imshow("松灵小车 - 深度图", depth_vis);
        }
        else
        {
            // 显示等待信息
            cv::Mat waiting_depth = cv::Mat::zeros(480, 640, CV_8UC3);
            cv::putText(waiting_depth, "等待深度图像数据...", 
                       cv::Point(150, 240), cv::FONT_HERSHEY_SIMPLEX, 1, 
                       cv::Scalar(0, 255, 255), 2);
            cv::imshow("松灵小车 - 深度图", waiting_depth);
        }
    }

    void run()
    {
        ros::Rate rate(30); // 30Hz
        
        while (ros::ok())
        {
            displayImages();
            
            // 检查按键
            char key = cv::waitKey(1);
            if (key == 'q' || key == 'Q')
            {
                ROS_INFO("用户请求退出");
                break;
            }
            
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "camera_display_node");
    
    // 设置控制台编码为UTF-8以正确显示中文
    setlocale(LC_ALL, "zh_CN.UTF-8");
    
    CameraDisplayNode node;
    node.run();
    
    return 0;
}
